/****************************************************************************
 *
 *   Copyright (C) 2019-2024 PX4 Development Team. All rights reserved.
 *   Adapted for FreeRTOS/STM32 by MicoAir Team.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

#include <px4_platform_common/intrusive_queue.h>
#include <string.h>

/**
 * @brief Get the node pointer from an item pointer
 */
static inline intrusive_queue_node_t *get_node(const intrusive_queue_t *queue, void *item)
{
    if (item == NULL) {
        return NULL;
    }
    return (intrusive_queue_node_t *)((uint8_t *)item + queue->node_offset);
}

/**
 * @brief Get the item pointer from a node pointer
 */
static inline void *get_item(const intrusive_queue_t *queue, intrusive_queue_node_t *node)
{
    if (node == NULL) {
        return NULL;
    }
    return (void *)((uint8_t *)node - queue->node_offset);
}

void intrusive_queue_init(intrusive_queue_t *queue, size_t node_offset)
{
    queue->head = NULL;
    queue->tail = NULL;
    queue->node_offset = node_offset;
}

bool intrusive_queue_empty(const intrusive_queue_t *queue)
{
    return queue->head == NULL;
}

void *intrusive_queue_front(const intrusive_queue_t *queue)
{
    return queue->head;
}

void *intrusive_queue_back(const intrusive_queue_t *queue)
{
    return queue->tail;
}

size_t intrusive_queue_size(const intrusive_queue_t *queue)
{
    size_t count = 0;
    void *item = queue->head;
    
    while (item != NULL) {
        count++;
        intrusive_queue_node_t *node = get_node(queue, item);
        item = get_item(queue, node->next);
    }
    
    return count;
}

void intrusive_queue_push(intrusive_queue_t *queue, void *item)
{
    if (item == NULL) {
        return;
    }
    
    intrusive_queue_node_t *new_node = get_node(queue, item);
    
    /* Error: node already queued or already inserted */
    if ((new_node->next != NULL) || (item == queue->tail)) {
        return;
    }
    
    /* If queue is empty, set head */
    if (queue->head == NULL) {
        queue->head = item;
    }
    
    /* If there's a tail, link it to the new node */
    if (queue->tail != NULL) {
        intrusive_queue_node_t *tail_node = get_node(queue, queue->tail);
        tail_node->next = new_node;
    }
    
    /* Update tail */
    queue->tail = item;
}

void *intrusive_queue_pop(intrusive_queue_t *queue)
{
    void *ret = queue->head;
    
    if (!intrusive_queue_empty(queue)) {
        intrusive_queue_node_t *head_node = get_node(queue, queue->head);
        
        if (queue->head != queue->tail) {
            /* More than one item in queue */
            queue->head = get_item(queue, head_node->next);
        } else {
            /* Only one item left */
            queue->head = NULL;
            queue->tail = NULL;
        }
        
        /* Clear next pointer in popped node (it might be re-inserted later) */
        head_node->next = NULL;
    }
    
    return ret;
}

bool intrusive_queue_remove(intrusive_queue_t *queue, void *item)
{
    if (item == NULL || intrusive_queue_empty(queue)) {
        return false;
    }
    
    /* Base case: removing head */
    if (item == queue->head) {
        intrusive_queue_node_t *head_node = get_node(queue, queue->head);
        
        if (head_node->next != NULL) {
            queue->head = get_item(queue, head_node->next);
            head_node->next = NULL;
        } else {
            queue->head = NULL;
            queue->tail = NULL;
        }
        
        return true;
    }
    
    /* Search for the item */
    void *current = queue->head;
    while (current != NULL) {
        intrusive_queue_node_t *current_node = get_node(queue, current);
        void *next_item = get_item(queue, current_node->next);
        
        /* Is the next item the one to remove? */
        if (next_item == item) {
            intrusive_queue_node_t *remove_node = get_node(queue, item);
            
            /* If removing tail, update tail pointer */
            if (item == queue->tail) {
                queue->tail = current;
            }
            
            /* Unlink the item */
            current_node->next = remove_node->next;
            remove_node->next = NULL;
            return true;
        }
        
        current = next_item;
    }
    
    return false;
}

